#ifndef KINEMATIC_H
#define KINEMATIC_H
#include <cmath>
#include <array>
#include "zhnmat.hpp"
#define _USE_MATH_DEFINES
#ifdef _MSVC_LANG
#define KINEMATIC_EXPORT extern "C" _declspec(dllexport)
#else
#define KINEMATIC_EXPORT
#endif

class RobotArm
{
public:
    enum FORWARD_MODE{
        TOTAL,
        JOINT5,
        JOINT6,
        EFFECTOR
    };

    RobotArm();
    ~RobotArm() {};
    void Forward_Solve(const double angle[], double posture[], FORWARD_MODE mode=TOTAL);
    int Inverse_Solve(const double posture[], double angle[]);
    std::array<int, 6> PulseCenter;  // 关节校准
    std::array<int, 6> PulseWorking;  // 工作准备
    std::array<int, 6> PulseSleep;  // 待机
    std::array<double, 6> NegRadLim;  // 触发传感器时的DH角
    std::vector<std::array<int, 6>> PulseCalibrate;  // 坐标系校准的固定点
    std::vector<std::array<int, 6>> PulseUser;  // 用户自定义固定点
    //每个步数对应的rad
    double DriveRatio[6];
    //末端点在关节6坐标系下的坐标
    //沿世界坐标系y轴向负方向看，平板竖直面对自己，笔在下，笔头朝右，仪器在右，6坐标系z轴指向自己，x向上，y向左
    zhnmat::Vector3d _Eff;
    // 用于轨迹规划
    double _Vmove;
    double _start[6], _end[6];
    double _totalT, _t=0;
    bool _isworking=false;
private:
    // 机械臂所有参数
    zhnmat::Mat R[6];
    zhnmat::Vector3d P[6];
    double L0;
    double L1;
    double L2;
    double L4;
    double L5;
};

typedef struct {
    double j0, j1, j2, j3, j4, j5;
}PythonJoint6;
KINEMATIC_EXPORT PythonJoint6 Python_Joint_Forward(PythonJoint6 a);
KINEMATIC_EXPORT PythonJoint6 Python_Joint_Inverse(PythonJoint6 a);

#endif
